//
// Created by xiang on 23-2-2.
//
#include "measure_sync.h"

namespace sad {

bool MessageSync::Sync() {
    bool is_used_odom = is_used_odom_;
    if(!is_used_odom) {
        if (lidar_buffer_.empty() || imu_buffer_.empty()) {
            return false;
        }
    } else {
        if (lidar_buffer_.empty() || imu_buffer_.empty() || odom_buffer_.empty()) {
            return false;
        }
    }


    if (!lidar_pushed_) {
        measures_.lidar_ = lidar_buffer_.front();
        measures_.lidar_begin_time_ = time_buffer_.front();

        lidar_end_time_ = measures_.lidar_begin_time_ + measures_.lidar_->points.back().time / double(1000);
        //LOG(INFO) << "lidar_end_time_ : " << measures_.lidar_begin_time_ << " , " << lidar_end_time_ << " , " << measures_.lidar_->points.back().time / double(1000);

        measures_.lidar_end_time_ = lidar_end_time_;
        lidar_pushed_ = true;
    }
    if(!is_used_odom) {
        if (last_timestamp_imu_ < lidar_end_time_) {
            return false;
        }        
    } else {
        if (last_timestamp_imu_ < lidar_end_time_ ||
            last_timestamp_odom_ < lidar_end_time_) {
            return false;
        }         
    }


    double imu_time = imu_buffer_.front()->timestamp_;
    measures_.imu_.clear();
    while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_)) {
        imu_time = imu_buffer_.front()->timestamp_;
        if (imu_time > lidar_end_time_) {
            break;
        }
        measures_.imu_.push_back(imu_buffer_.front());
        imu_buffer_.pop_front();
    }
    
    //LOG(INFO) << "measures_.imu_ : " << measures_.imu_.size();

    if(is_used_odom) {
        double odom_time = odom_buffer_.front()->timestamp_;
        measures_.odom_.clear();
        while ((!odom_buffer_.empty()) && (odom_time < lidar_end_time_)) {
            odom_time = odom_buffer_.front()->timestamp_;
            if (odom_time > lidar_end_time_) {
                break;
            }
            measures_.odom_.push_back(odom_buffer_.front());
            odom_buffer_.pop_front();
        }        
    }

    //LOG(INFO) << "measures_.odom_ : " << measures_.odom_.size();
    //LOG(INFO) << "measures_.lidar_ : " << measures_.lidar_.size();

    //for(int i = 0; i < measures_.imu_.size();i++) {
        //LOG(INFO) << "imu_time : " << measures_.imu_[i]->timestamp_;
    //}
    //for(int i = 0; i < measures_.odom_.size();i++) {
        //LOG(INFO) << "odom_time : " << measures_.odom_[i]->timestamp_;
    //}

    lidar_buffer_.pop_front();
    time_buffer_.pop_front();
    lidar_pushed_ = false;

    if (callback_) {
        callback_(measures_);
    }

    return true;
}

void MessageSync::Init(const std::string& yaml) { conv_->LoadFromYAML(yaml); }

}  // namespace sad